#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import action_point as ap
import local_pyhop as ph
import methods_point
import sys, termios,time



def __init__():
    rospy.init_node('Dynamic_aera', anonymous=True)
    print('Vehicle initialized complete!')

if __name__=="__main__":
    __init__()
    rate = rospy.Rate(10)
    tasks = ap.get_infor()
    state = ph.State('state_now')
    i = 0
    while True:
        state = ap.get_state(state,i)
        i+=1
        tasks,plan = ph.pyhop(state, tasks, verbose=2)
        print('Vehicle start to execute the executable plan!')
        time.sleep(2)
        for j in range(len(plan)):
            parameters = plan[j][1:]
            getattr(ap, plan[j][0])(rate, *parameters)
            if plan[j][0] == 'scan' and ap.aruco_info.detected == True:
                tasks = [('return_land_task', state.pos_land)]
                break
        if tasks == [] and ap.aruco_info.detected == True:
            print("Execute task over!")
            break
        if tasks == [] and ap.aruco_info.detected == False:
            print("No target found in the area!\nExecute task over!\n")
            break
